#!/usr/bin/python
# coding=utf-8
import os
import rospy
import tf
import time
import sys
import math
import serial
import string
from std_msgs.msg import Int8
import ctypes

#class queue is design for uart receive data cache
class queue:
    def __init__(self, capacity = 1024*4):
        self.capacity = capacity
        self.size = 0
        self.front = 0
        self.rear = 0
        self.array = [0]*capacity
 
    def is_empty(self):
        return 0 == self.size
 
    def is_full(self):
        return self.size == self.capacity
 
    def enqueue(self, element):
        if self.is_full():
            raise Exception('queue is full')
        self.array[self.rear] = element
        self.size += 1
        self.rear = (self.rear + 1) % self.capacity
 
    def dequeue(self):
        if self.is_empty():
            raise Exception('queue is empty')
        self.size -= 1
        self.front = (self.front + 1) % self.capacity
 
    def get_front(self):
        return self.array[self.front]
    
    def get_front_second(self):
        return self.array[((self.front + 1) % self.capacity)]

    def get_queue_length(self):
        return (self.rear - self.front + self.capacity) % self.capacity

    def show_queue(self):
        for i in range(self.capacity):
            print self.array[i],
        print(' ')

#class BaseControl:
class CleanControl:
    def __init__(self):
        #Get params
        self.baseId = rospy.get_param('~base_id','base_footprint')
        self.device_port = rospy.get_param('~port','/dev/clean-usb')
        self.baudrate = int(rospy.get_param('~baudrate','115200'))
        self.clean_freq = int(rospy.get_param('~clean_freq','20'))
        self.clean_cmd_topic = rospy.get_param('~clean_cmd_topic','/clean_cmd')
        self.clean_report_topic = rospy.get_param('~clean_report_topic','/clean_report')

        #define param
        self.current_time = rospy.Time.now()
        self.previous_time = self.current_time
        self.last_cmd_vel_time = rospy.Time.now()
        self.status = 9 # 0 Ironing board close, 1 Ironing board1, 2 Ironing board2, 3 forward cleaning, 4 reverse cleaning, 5 stop cleaning, 6 restart 
        self.clean_report_status = 0 # 0 Normal, 1 Finish Cleaning, 2 Stop Moving, 3 Moving
        self.Circleloop = queue(capacity = 1024 *4)
        # Serial Communication
        try:
            self.serial = serial.Serial(self.device_port,self.baudrate,timeout=10)
            rospy.loginfo("Opening Serial")
            try:
                if self.serial.in_waiting:
                    self.serial.readall()
            except:
                rospy.loginfo("Opening Serial Try Faild")
                pass
        except:
            rospy.logerr("Can not open Serial"+self.device_port)
            self.serial.close
            sys.exit(0)
        rospy.loginfo("Serial Open Succeed")
        #if move base type is ackermann car like robot and use ackermann msg ,sud ackermann topic,else sub cmd_vel topic
        self.sub = rospy.Subscriber(self.clean_cmd_topic,Int8,self.cmdMain,queue_size=10)
        self.pub = rospy.Publisher(self.clean_report_topic,Int8,queue_size=10)
        self.timer_communication = rospy.Timer(rospy.Duration(1.0/20),self.timerCommunicationCB)


    #CRC-8 Calculate
    def crc_1byte(self,data):
        crc_1byte = 0
        for i in range(0,8):
            if((crc_1byte^data)&0x01):
                crc_1byte^=0x18
                crc_1byte>>=1
                crc_1byte|=0x80
            else:
                crc_1byte>>=1
            data>>=1
        return crc_1byte
        
    def crc_byte(self,data,length):
        ret = 0
        for i in range(length):
            ret = self.crc_1byte(ret^data[i])
        return ret

    #Subscribe clean_status call this to send clean cmd to clean-usb
    def cmdMain(self,msg):
        self.status = msg.data
        rospy.loginfo("command data:%d" %msg.data)
        if self.status == 0 or self.status == 1 or self.status == 2:
            self.cmdIroning()
        elif self.status == 3 or self.status == 4 or self.status == 5:
            self.cmdClean()
        elif self.status == 6:
            self.cmdRestart()

    def cmdIroning(self):
        ironing_flag = 0
        if self.status == 1 or self.status == 2:
            ironing_flag = 1 # Ironing board 0x01
        output = chr(0x5a) + chr(0x08) + chr(0x00) + chr(0x01) + \
                chr(ironing_flag&0xff) + chr(self.status&0xff) + \
                chr(0x00)
        output += chr(0xff)
        outputHex = 0
        for i in output:
            d = ord(i) 
            outputHex = outputHex*256 + d
        rospy.loginfo("Send down cmdIroning command:%x" %outputHex)
        try:
            while self.serial.out_waiting:
                pass
            self.serial.write(output)
        except:
            rospy.logerr("Ironing Board Command Send Faild")

    def cmdClean(self):
        cleaning_flag = 0
        cleaning_direction = 0
        if self.status == 3: # forward cleaning 0x03 0x01
            cleaning_flag = 3
            cleaning_direction = 1
        elif self.status == 4: # reverse cleaning 0x03 0x02
            cleaning_flag = 3
            cleaning_direction = 2
        if self.status == 5: #stop cleaning 0x07 0x01
            cleaning_flag = 7
            cleaning_direction = 1
        output = chr(0x5a) + chr(0x07) + chr(0x00) +chr(cleaning_flag&0xff) + \
                chr(cleaning_direction&0xff) + \
                chr(0x00)
        output += chr(0xff)
        outputHex = 0
        for i in output:
            d = ord(i) 
            outputHex = outputHex*256 + d
        rospy.loginfo("Send down cmdClean command:%x" %outputHex)
        try:
            while self.serial.out_waiting:
                pass
            self.serial.write(output)
        except:
            rospy.logerr("Cleaning Command Send Faild")

    def cmdRestart(self):
        restart_flag = 0x00
        if self.status == 6:
            restart_flag = 0x0b
        output = chr(0x5a) + chr(0x07) + chr(0x00) + chr(restart_flag) + \
                chr(0x01) + chr(0x00)
        output += chr(0xff)
        outputHex = 0
        for i in output:
            d = ord(i) 
            outputHex = outputHex*256 + d
        rospy.loginfo("Send down cmdRestart command:%x" %outputHex)
        try:
            while self.serial.out_waiting:
                pass
            self.serial.write(output)
        except:
            rospy.logerr("Restart Command Send Faild")

    #Communication Timer callback to handle receive data
    #depend on communication protocol
    def timerCommunicationCB(self,event):
        # Send Connection Command
        output = chr(0x5a) + chr(0x07) + chr(0x00) + chr(0x0d) + chr(0x01) + chr(0x00) + chr(0x64)
        # while(self.serialIDLE_flag):
        #     time.sleep(0.01)
        # self.serialIDLE_flag = 1
        try:
            while self.serial.out_waiting:
                pass
            self.serial.write(output)
        except:
            rospy.logerr("Connection Command Send Faild")
        # Receive Information
        length = self.serial.in_waiting
        if length:
            reading = self.serial.read_all()
            if len(reading)!=0:
                for i in range(0,len(reading)):
                    data = (int(reading[i].encode('hex'),16)) 
                    try:
                        self.Circleloop.enqueue(data)
                    except:
                        rospy.logerr("Circleloop.enqueue Faild")
        else:
            pass
        if self.Circleloop.is_empty()==False:
            if self.Circleloop.is_empty()==False:
                data = self.Circleloop.get_front()
            else:
                pass
            if data == 0x5a:
                length = self.Circleloop.get_front_second()
                if length > 1 :
                    if self.Circleloop.get_front_second() <= self.Circleloop.get_queue_length():
                        databuf = []
                        for i in range(length):
                            databuf.append(self.Circleloop.get_front())
                            self.Circleloop.dequeue()
                        if (databuf[length-1]) == self.crc_byte(databuf,length-1):
                            pass
                        else:
                            # print databuf
                            # print "Crc check Err %d"%self.crc_byte(databuf,length-1)
                            pass
                        #parse receive data
                        databufhex= ""
                        for data in databuf:
                            databufhex += hex(data) + " "
                        # rospy.loginfo("Up clean command:%s" %databufhex)
                        if(databuf[3] == 0x02):     
                            if (databuf[4] == 0x01): 
                                self.clean_report_status = 1 #清洁板收到烫板命令
                        elif(databuf[3] == 0x04):   
                            if (databuf[4] == 0x01):
                                self.clean_report_status = 2 #清洁板收到开始清洁命令
                        elif(databuf[3] == 0x05):     
                            if (databuf[4] == 0x01): 
                                self.clean_report_status = 3  #清洁板上报清洁结束
                        elif(databuf[3] == 0x08):     
                            if (databuf[4] == 0x01): 
                                self.clean_report_status = 4  #清洁板收到强制结束清洁命令
                        elif(databuf[3] == 0x09):       
                            flag = databuf[4]
                            if flag == 0:
                                self.clean_report_status = 5     #清洁板上报请求停止运动
                            elif flag == 1:
                                self.clean_report_status = 6     #清洁板上报请求运动
                        else:
                            pass
                else:
                    pass
            else:
                self.Circleloop.dequeue()
        else:
            # rospy.loginfo("Circle is Empty")
            pass
        # rospy.loginfo("self.clean_report_status:%s" %self.clean_report_status)
        self.pub.publish(self.clean_report_status)
        
#main functionc'
if __name__=="__main__":
    try:
        rospy.init_node('clean_control',anonymous=True)
        bc = CleanControl()
        rospy.spin()
    except KeyboardInterrupt:
        bc.serial.close
        print("Shutting down")
